///////////////////////////////////////////////////////////////////////////
//
// Copyright (c) 2025, STEREOLABS.
//
// All rights reserved.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//
///////////////////////////////////////////////////////////////////////////

#include <sl/c_api/zed_interface.h>
#include <stdbool.h>
#include <stdio.h>

int main(int argc, char** argv) {

    // Create a ZED camera object
    int camera_id = 0;
    sl_create_camera(camera_id);

    struct SL_InitParameters init_param;
    init_param.camera_fps = 30;
    init_param.resolution = SL_RESOLUTION_HD1080;
    init_param.input_type = SL_INPUT_TYPE_USB;
    init_param.camera_device_id = camera_id;
    init_param.camera_image_flip = SL_FLIP_MODE_AUTO;
    init_param.camera_disable_self_calib = false;
    init_param.enable_image_enhancement = true;
    init_param.svo_real_time_mode = true;
    init_param.depth_mode = SL_DEPTH_MODE_NEURAL;
    init_param.depth_stabilization = 30;
    init_param.depth_maximum_distance = 40;
    init_param.depth_minimum_distance = -1;
    init_param.coordinate_unit = SL_UNIT_METER;
    init_param.coordinate_system = SL_COORDINATE_SYSTEM_LEFT_HANDED_Y_UP;
    init_param.sdk_gpu_id = -1;
    init_param.sdk_verbose = false;
    init_param.sensors_required = false;
    init_param.enable_right_side_measure = false;
    init_param.open_timeout_sec = 5.0f;
    init_param.async_grab_camera_recovery = false;
    init_param.grab_compute_capping_fps = 0;
    init_param.enable_image_validity_check = false;

    // Open the camera
    int state = sl_open_camera(camera_id, &init_param, 0, "", "", 0, "", "", "");

    if (state != 0) {
        printf("Error Open \n");
        return 1;
    }

    struct SL_RuntimeParameters rt_param;
    rt_param.enable_depth = true;
    rt_param.confidence_threshold = 95;
    rt_param.reference_frame = SL_REFERENCE_FRAME_CAMERA;
    rt_param.texture_confidence_threshold = 100;
    rt_param.remove_saturated_areas = true;

    int width = sl_get_width(camera_id);
    int height = sl_get_height(camera_id);

    // Create image ptr.
    int* image_ptr;
    // Init pointer.
    image_ptr = sl_mat_create_new(width, height, SL_MAT_TYPE_U8_C4, SL_MEM_CPU);

    // Capture 50 frames and stop
    int i = 0;
    while (i < 50) {
        // Grab an image
        state = sl_grab(camera_id, &rt_param);
        // A new image is available if grab() returns ERROR_CODE::SUCCESS
        if (state == 0) {

            // Get the left image
            sl_retrieve_image(camera_id, image_ptr, SL_VIEW_LEFT, SL_MEM_CPU, width, height);

            // Display the image resolution and its acquisition timestamp
            printf("Image resolution: %i x %i || %llu \n", width, height, sl_get_current_timestamp(camera_id));
            i++;
        }
    }

    sl_close_camera(camera_id);
    return 0;
}
